import cv2
import numpy as np
class Apriltag(object):
    def __init__(self):
        self.tagfamily = None
        self.tagdetector = None

    def create_detector(self,sigma=0.8,nthread =1,debug = False,minarea = 400,thresholding = 'adaptive',downsampling = False):
        '''
        init what kind of tag you will detect
        '''
        self._downsampling = downsampling
        self._quad_sigma = sigma
        self._nthread = nthread
        self._minarea = minarea
        self._debug = debug
        self._thresholding = thresholding



    def detect(self,frame):
        gray = np.array(cv2.cvtColor(frame,cv2.COLOR_RGB2GRAY))
        """
        1 blur
        """
        img = cv2.GaussianBlur(gray, (3, 3), self._quad_sigma)

        """
        2 adaptive thresholding or  canny
        """
        if(self._thresholding=='canny'):
            img = cv2.Canny(img,50,350,apertureSize=3)
        elif(self._thresholding=='adaptive'):
            img = np.array(cv2.adaptiveThreshold(img, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY_INV, 9, 5),dtype='uint8')
            kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(2,2))
            img = cv2.morphologyEx(img, cv2.MORPH_OPEN,kernel)
#           img = cv2.GaussianBlur(img, (7, 7), self._quad_sigma)

        """
        3 find contours
        """
        contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        """
        4 compute convex hulls and find maximum inscribed quadrilaterals
        """
        quads = [] # array of quad including four peak points
        quads_area  = []
        for i in range(len(contours)):
            if (hierarchy[0, i, 3] < 0 and contours[i].shape[0] >= 4):
                area = cv2.contourArea(contours[i])
                if area > self._minarea:
                    hull = cv2.convexHull(contours[i])
                    if (area / cv2.contourArea(hull) > 0.8):
                        quad = cv2.approxPolyDP(hull, 8, True) # maximum_area_inscribed
                        if (len(quad) == 4):
                            areaqued = cv2.contourArea(quad)
                            areahull = cv2.contourArea(hull)
                            if areaqued / areahull > 0.8 and areahull >= areaqued:
                                quads.append(quad)
                                quads_area.append(area)
        # quad = self.tagfamily.decodeQuad(quads,gray)
        quad = quads[int(np.argmax(np.array(quads_area)))].reshape(-1,2) # 选择区域最大的

        return quad

if __name__ == '__main__':
    detector = Apriltag()
    cv2.namedWindow("out",cv2.WINDOW_NORMAL)
    cv2.resizeWindow("out",640,380)
    img = cv2.imread("c2.jpg")
    detector.create_detector()
    quad = detector.detect(img)
    for p in quad:
        img = cv2.circle(img,p,10,(0,255,0),-1)
    cv2.imshow("out",img)
    cv2.waitKey(0)